#include <Servo.h>
#include <ServoGyro.h>

ServoGyro servo1;

void setup() {

  int digitalPin = 6;
  int analogPin = 0;
  
  pinMode(digitalPin,OUTPUT);
  
  servo1.attach(6); //digital pin 6
  analogReference(EXTERNAL);
  
  // after setting analog reference, the first couple of measurements on analogin can be inaccurate
  for(int i = 0; i<50; i++) {
      analogRead(analogPin);
      delay(5);
  }
  
  Serial.begin(9600);
  Serial.println("Ready");
  
  servo1.calibrate(analogPin,digitalPin, 250, 50);
  
// JP's old servos, with analogReference(EXTERNAL);
// int minVoltage = 126
// int maxVoltage = 517
// int minPulseWidth = 627
// int maxPulseWidth = 2392

// JP's Hitec HS-81
// int minVoltage = 80;
// int maxVoltage = 354;
// int minPulseWidth = 627;
// int maxPulseWidth = 2392;

}

void loop() {

  static int v = 0;
  //int flag = 1;
  int analogPin = 5;
  int minValVolt;
  int minValPWM;
  int maxValVolt;
  int maxValPWM;

  //int servoOffset = 150;
  
//  servo1.goToMin(5);
//  servo1.writeMicroseconds(MIN_PULSE_WIDTH);
//  delay(1500);
//  servo1.writeMicroseconds(MAX_PULSE_WIDTH);
//  servo1.goToMax(5);
//  delay(1500);

//  // calibration parameter
//  int analog_voltage_prev = 0;
//  int pwm_width_prev = 0;
//  int calibration_step = 250;
//  int calibration_break_threshold = 50;
//  
//  
//    SERVO CALIBRATION TEST CODE
//  if(servo1.attached()) {
//      // move servo to center position
//      servo1.writeMicroseconds((MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2);
//      delay(2000);
//      
//      // find minimum position, analog voltage and pwm pulse width
//      analog_voltage_prev = analogRead(analogPin) + 5;
//      pwm_width_prev = (MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2;
//      Serial.print("Current Voltage: "); Serial.println(analog_voltage_prev);
//      
//      while((abs((int)analogRead(analogPin) - analog_voltage_prev)) >= 1) {
//          analog_voltage_prev = analogRead(analogPin);
//          pwm_width_prev = pwm_width_prev - 15;
//          servo1.writeMicroseconds(pwm_width_prev);
//          delay(calibration_step);
//          
//          Serial.print("Previous Voltage / Current Voltage / Diff: "); 
//              Serial.print(analogRead(analogPin)); Serial.print(" / "); 
//              Serial.print(analog_voltage_prev); Serial.print(" / "); 
//              Serial.println(abs(analogRead(analogPin) - analog_voltage_prev));
//              
//          //if((float)analog_voltage_prev > (float)analogRead(analogPin) || (float)(abs(analogRead(analogPin) - analog_voltage_prev)) > (float)10) {
//          if((abs((int)analogRead(analogPin) - analog_voltage_prev)) > calibration_break_threshold) {
//              break;
//          }
//      }
//      
//      minValVolt = analog_voltage_prev;
//      minValPWM = pwm_width_prev + 10;
//      Serial.print("Min Voltage: "); Serial.println(analog_voltage_prev); 
//      Serial.print("Min PWM Pulse Width: "); Serial.println(pwm_width_prev);
//      
//      // move servo to center position
//      servo1.writeMicroseconds((MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2);
//      delay(1000);
//      
//      // find minimum position, analog voltage and pwm pulse width
//      analog_voltage_prev = analogRead(analogPin) + 5;
//      pwm_width_prev = (MIN_PULSE_WIDTH+MAX_PULSE_WIDTH)/2;
//      Serial.print("Current Voltage: "); Serial.println(analog_voltage_prev);
//      
//      while((abs((int)analogRead(analogPin) - analog_voltage_prev)) >= 1) {
//          analog_voltage_prev = analogRead(analogPin);
//          pwm_width_prev = pwm_width_prev + 15;
//          servo1.writeMicroseconds(pwm_width_prev);
//          delay(calibration_step);
//          Serial.print("Previous Voltage / Current Voltage / Diff: "); 
//              Serial.print(analogRead(analogPin)); Serial.print(" / "); 
//              Serial.print(analog_voltage_prev); Serial.print(" / "); 
//              Serial.println(abs(analogRead(analogPin) - analog_voltage_prev));
//              
//          //if((float)analog_voltage_prev > (float)analogRead(analogPin) || (float)(abs(analogRead(analogPin) - analog_voltage_prev)) > (float)10) {
//          if((abs((int)analogRead(analogPin) - analog_voltage_prev)) > calibration_break_threshold) {
//              break;
//          }
//      }
//      
//      maxValVolt = analog_voltage_prev;
//      maxValPWM = pwm_width_prev - 10;
//      Serial.print("Max Voltage: "); Serial.println(analog_voltage_prev); 
//      Serial.print("Max PWM Pulse Width: "); Serial.println(pwm_width_prev);
//      
//      servo1.writeMicroseconds(minValPWM);
//      delay(1500);
//      servo1.writeMicroseconds(maxValPWM);
//      delay(1500);
      
//    OLD SERVO TEST CODE
//      servo1.writeMicroseconds(MIN_PULSE_WIDTH + servoOffset);
//      delay(2000);
//      minValVolt = analogRead(analogPin);
//      Serial.print("Min Voltage: "); Serial.println(analogRead(analogPin));
//      servo1.writeMicroseconds(MAX_PULSE_WIDTH);
//      delay(2000);
//      maxValVolt = analogRead(analogPin);
//      Serial.print("Max Voltage: "); Serial.println(analogRead(analogPin));
//      servo1.writeMicroseconds(MIN_PULSE_WIDTH + servoOffset);
//      delay(2000);
//      
//      Serial.print("Servo Angle: "); Serial.println(map(analogRead(analogPin), minValVolt, maxValVolt, 0, 180));
//      
//      // map angle to pwm width
//      
//      while(servo1.readMicroseconds() < MAX_PULSE_WIDTH) {
//          servo1.writeMicroseconds(servo1.readMicroseconds() + 20);
//          delay(500);
//          Serial.print("Servo Val: "); Serial.println(analogRead(analogPin));
//          Serial.print("Servo Angle: "); Serial.println(map(analogRead(analogPin), minValVolt, maxValVolt, 0, 180));
//      }
//  }
} 
